#include "ros/ros.h"
#include "jsk_recognition_msgs/BoundingBoxArray.h"
#include "std_msgs/Float64MultiArray.h"
#include <cmath>
#define DEBUG 1
#define TO_FILE DEBUG and 0
#if DEBUG
#include <iostream>
#endif
#if TO_FILE
#include <fstream>
#include <streambuf>
#include <stdlib.h>
#include <stdio.h>
#endif
double phi = M_PI_2;
void lidar_callback(const jsk_recognition_msgs::BoundingBoxArray& lidar)noexcept(true)
{
#if DEBUG
    std::cout << __func__ << '\n';
#endif
    std::pair<const jsk_recognition_msgs::BoundingBox*, const jsk_recognition_msgs::BoundingBox*> mate[4] = 
    {
        {nullptr, nullptr}, {nullptr, nullptr}, {nullptr, nullptr}, {nullptr, nullptr} 
    };
	for (const auto& p : lidar.boxes)
		if (p.pose.position.x > -0.5 and p.pose.position.x < 16 and std::abs(p.pose.position.y) < 4.5)
		{
			double i = (p.pose.position.x - 0.3) / 5.0;
			int index = std::round(i);
			if (std::abs(i - index) < 0.2)
                if (mate[index].second)
                {
                    const double abs_p_y = std::abs(p.pose.position.y);
                    if (abs_p_y < std::abs(mate[index].second->pose.position.y))
                         if (abs_p_y < std::abs(mate[index].first->pose.position.y))
                         {
                             mate[index].second = mate[index].first;
                             mate[index].first = &p;
                         }
                         else
                            mate[index].second = &p;
                }
                else if (mate[index].first)
                    if (std::abs(p.pose.position.y) < std::abs(mate[index].first->pose.position.y))
                    {
                        mate[index].second = mate[index].first;
                        mate[index].first = &p;
                    }
                    else
                        mate[index].second = &p;
                else
                    mate[index].first = &p;
		}
	int count = 0;
	double k = 0;
	for (const auto& p : mate)
		if (p.first and p.second)
		{
			k -= (p.second->pose.position.x - p.first->pose.position.x) / (p.second->pose.position.y - p.first->pose.position.y);
			++count;
#if DEBUG
            std::cout << "检测到的第" << count << "对桶的坐标是(" << p.first->pose.position.x << ", " << p.first->pose.position.y << "), ("
                               << p.second->pose.position.x << ", " << p.second->pose.position.y << "), 1 / k = "
                               << (p.second->pose.position.x - p.first->pose.position.x) / (p.second->pose.position.y - p.first->pose.position.y) << '\n';
#endif
		}
    if (count not_eq 0)
	    phi = std::atan(k / count);
#if DEBUG
    static int test_count;
    std::cout << "第" << test_count << "次求初始航向角phi，phi = " << phi * 180 / M_PI << "度\n";
    ++test_count;
#endif
}
int main(int argc, char **argv)noexcept(true)
{
////////////////////////////////////////////////////////////////////////
    //std::cout << "phi = ";
    //std::cin >> phi;
    //phi *= M_PI / 180;
////////////////////////////////////////////////////////////////////////
    ros::init(argc, argv, "line_path");
    ros::NodeHandle n;
#if TO_FILE
    std::streambuf* cout_buffer(std::cout.rdbuf());
    char path[200];
    realpath("debug.log", path);
    std::cout << path << '\n';
    std::ofstream out(path, std::ios::app);
    std::cout.rdbuf(out.rdbuf());
#endif
    {//限制subscriber的生存期
        constexpr double phi_limit = M_PI / 18;//10度
#if DEBUG
        std::cout << "检测到角度在±" << phi_limit * 180 / M_PI << "°或以内时停止检测\n";
#endif
        ros::Subscriber sub(n.subscribe("/detected_bounding_box", 20, lidar_callback));
        while (std::abs(phi) > phi_limit)//do while
            ros::spinOnce();
    }
    // const double p = phi / 2.3;
    constexpr double frequency = 5;//10;
    constexpr double radian_to_angle = 180 / M_PI;
    const double tan_phi = std::tan(phi);
    constexpr double double_frequency = frequency + frequency;
    constexpr double delta_v = 0.5;//frequency;
    constexpr double length = 1.511;
    constexpr double double_length = length + length;
    constexpr double v_max = 1;
    struct xi
    {
        double x, y, yaw;
        double v, delta;
    }state{0, 0, phi, 0, 0};
    ros::Rate loop_rate(frequency);
    ros::Publisher pub(n.advertise<std_msgs::Float64MultiArray>("/command", 120));
    std_msgs::Float64MultiArray msg;
    msg.data.resize(5);
    msg.data[2] = 0;//不紧急制动
    msg.data[3] = 1;//制动等级，目前没用
    while (true)//std::abs(state.yaw) > 1e-5)
    {
        double v = state.v + delta_v;
        if (v > v_max)
            v = v_max;
        const double sum_v = state.v + v;
        const double s = sum_v / double_frequency;
        const double delta_x = s * std::cos(state.yaw);
        const double x = state.x + delta_x;
        // double tan_delta_devided_by_length = l / std::tan(delta);
        // state.x += tan_delta_devided_by_length * std::sin((state.v + 0.5 * delta_v) / frequency / tan_delta_devided_by_length);
        const double y = tan_phi * (1 - std::exp(-x));
        const double delta_y = y - state.y;
        state.delta = std::atan(double_length * std::sin(std::atan2(delta_y, delta_x) - state.yaw) / std::sqrt(delta_y * delta_y + delta_x * delta_x));// + p;
        const double yaw = state.yaw + s * std::tan(state.delta) / length;
        if (std::abs(yaw) >= std::abs(state.yaw))
            break;
        // state.x += s * std::abs((std::sin(state.yaw) - std::sin(state.yaw - state.delta)) / state.delta);
        state.x = x;
        state.y = y;
        msg.data[0] = state.v = v;
        state.yaw = yaw;
        msg.data[1] = state.delta * radian_to_angle;
#if DEBUG
        std::cout << "y = " << y << ",\tdelta = " << state.delta << ",\tyaw = " << state.yaw << '\n';
#endif
        pub.publish(msg);
        loop_rate.sleep();
    }
#if  DEBUG
    std::cout << "调整完成\n";
#endif
    constexpr double delta_x = v_max / frequency;
    for(msg.data[0] = v_max, msg.data[1] = 0; state.x < 5; state.x += delta_x)//90
    {
        pub.publish(msg);
        loop_rate.sleep();
    }
#if DEBUG
    std::cout << "75m线\n";
#endif
    for (msg.data[0] = 0; state.x < 15; state.x += delta_x)//115
    {
        pub.publish(msg);
        loop_rate.sleep();
    }
    msg.data[4] = 3;//完成
    pub.publish(msg);
#if DEBUG
    std::cout << "------------完赛--------------\n";
#endif
#if TO_FILE
    std::cout.rdbuf(cout_buffer);
#endif
}
#undef TO_FILE
#undef DEBUG
